#include "MeasurE616AlgFrontSe1ToSe6Edge.h"

MeasurE616AlgFrontSE1toSE6Edge::MeasurE616AlgFrontSE1toSE6Edge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgFrontSE1toSE6Edge::operator()(cv::Mat &srcImg, int id,std::map<string, float> &measRes, int fixtureId)
{
    //front SE123456-> 28,29,30,31,32,33
    std::vector<std::vector<cv::Point2f>>resptVec;
    resptVec.resize(6);
    for(auto i=28;i<34;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-28,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj. NewKirschEdgeOuter(imgt,tparas,contours);

        ImageCoordinateTransformation(contours,rangVec);
        if(i==29)
        {
            std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
            while (!MeasurE616AlgBase::flagVec[id])
            {
                (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[1].push_back(ptsOut);
            }
        }
        else
        {
//            std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrfrontVec[id]);
//            while (!MeasurE616AlgBase::flagfrontVec[id])
//            {
//                (*MeasurE616AlgBase::cvarptrfrontVec[id]).wait(lock);
//            }
//            for(auto &elem:contours)
//            {
//                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumFrontVec[id],ptsIn,ptsOut);
//                resptVec[i-28].push_back(ptsOut);
//            }
            std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
            while (!MeasurE616AlgBase::flagVec[id])
            {
                (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem : contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec[i-28].push_back(ptsOut);
            }
        }
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI44-SE1-Y1-distance_pixel"), resptVec[0][0].y));
    float fai44sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI44-SE1-Y1-distance")], resptVec[0][0].y);
    measRes.insert(std::make_pair(string("FAI44-SE1-Y1-distance_real"), fai44sey));

    measRes.insert(std::make_pair(string("FAI44-SE1-Y2-distance_pixel"), resptVec[0][1].y));
    float fai44sey2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI44-SE1-Y2-distance")], resptVec[0][1].y);
    measRes.insert(std::make_pair(string("FAI44-SE1-Y2-distance_real"), fai44sey2));

    float fai47=0;
    if(resptVec[1].size()!=0){
        for(int i=0;i<resptVec[1].size();++i){
            fai47+=resptVec[1][i].x/resptVec[1].size();
        }
    }
    measRes.insert(std::make_pair(string("FAI47-SE2-X-distance_pixel"), fai47));
    float fai47sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI47-SE2-X-distance")], fai47);
    measRes.insert(std::make_pair(string("FAI47-SE2-X-distance_real"), fai47sex));

    float fai48=0;
    if(resptVec[2].size()!=0){
        for(int i=0;i<resptVec[2].size();++i){
            fai48+=resptVec[2][i].x/resptVec[2].size();
        }
    }
    measRes.insert(std::make_pair(string("FAI48-SE3-X-distance_pixel"), fai48));
    float fai48sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI48-SE3-X-distance")], fai48);
    measRes.insert(std::make_pair(string("FAI48-SE3-X-distance_real"), fai48sex));

    measRes.insert(std::make_pair(string("FAI66-SE4-Y-distance_pixel"), resptVec[3][0].y));
    float fai66sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI66-SE4-Y-distance")], resptVec[3][0].y);
    measRes.insert(std::make_pair(string("FAI66-SE4-Y-distance_real"), fai66sey));

    float fai72=0;
    if(resptVec[4].size()!=0){
        for(int i=0;i<resptVec[4].size();++i){
            fai72+=resptVec[4][i].x/resptVec[4].size();
        }
    }
    measRes.insert(std::make_pair(string("FAI72-SE5-X-distance_pixel"), fai72));
    float fai72sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI72-SE5-X-distance")], fai72);
    measRes.insert(std::make_pair(string("FAI72-SE5-X-distance_real"), fai72sex));

    float fai49=0;
    if(resptVec[5].size()!=0){
        for(int i=0;i<resptVec[5].size();++i){
            fai49+=resptVec[5][i].y/resptVec[5].size();
        }
    }
    measRes.insert(std::make_pair(string("FAI49-SE6-Y-distance_pixel"), fai49));
    float fai49sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI49-SE6-Y-distance")], fai49);
    measRes.insert(std::make_pair(string("FAI49-SE6-Y-distance_real"), fai49sey));
}

